#include "device.h"
#include "chassic.h"
#include "ps2_remote_control.h"
#include "grayscale.h"

/***************************************数据定义区***************************************/




extern TIM_HandleTypeDef htim2;                        //定时器2初始化句柄

int Left_Speed, Right_Speed;


void track(int pwm_left, int pwm_right)                                  //自动循迹函数
{
//int integral;
  int error;
  int dir;
  int kp = 100;
  int pid = 0;
  if (((left_value == 0) || (right_value == 0)) && (ps2_data.red == 0))  //左右灰度传感器检测到停止线且红色按键未被按下时小车停止
  {
//    motor_speed_l(0);
//    motor_speed_r(0);
  } else
  {
    if ((ps2_data.green == 1) && (ps2_data.blue == 0))                 //按下绿色按键同时松开蓝色按键小车向前循迹
    {
      kp = 100;
      dir = 1;
      switch (front_value)
      {
        case 0x01:          //偏右
          error = 1;
          break;
        case 0x03:          //偏右+
          error = 2;
          break;
        case 0x04:          //偏左
          error = -1;
          break;
        case 0x06:          //偏左+
          error = -2;
          break;
        default:
          error = 0;
          break;
      }
    } else if ((ps2_data.green == 0) && (ps2_data.blue == 1))           //按下蓝色按键同时松开绿色按键小车向前循迹
    {
      kp = -100;
      dir = -1;
      switch (back_value)
      {
        case 0x01:          //偏右
          error = 1;
          break;
        case 0x03:          //偏右+
          error = 2;
          break;
        case 0x04:          //偏左
          error = -1;
          break;
        case 0x06:          //偏左+
          error = -2;
          break;
        default:
          error = 0;
          break;
      }
    } else
    {
      kp = 0;
      dir = 0;
    }
    pid = kp * error;
//    motor_speed_l(dir * pwm_left + pid);
//    motor_speed_r(dir * pwm_right + pid);
  }
}


void chassis_control(void)
{
  //左右轮速度解算
  Right_Speed = -(ps2_data.ly - 2 * ps2_data.lx);
  Left_Speed = +(ps2_data.ly + 2 * ps2_data.lx);

//  motor_speed_l(Left_Speed);
//  motor_speed_r(Right_Speed);
}


